#ifndef MAPPOINTS_COST_FUNCTION_H
#define MAPPOINTS_COST_FUNCTION_H

#include "MapPoints.h"
#include "ceres/ceres.h"

class MappointsCostFunction{

public:
    MappointsCostFunction(double observation_x, double observation_y, Eigen::Isometry3d camera):
            observed_x(observation_x),observed_y(observation_y){
        pose_c = camera;
    }

    template<typename T>
    bool operator()(const T* const point,
                    T* residuals)const{
        T point3d[3];
        Eigen::Matrix3d rotation = pose_c.rotation();
        Eigen::Vector3d translation = pose_c.translation();

        point3d[0] =rotation(0,0)*point[0]+rotation(0,1)*point[1]+rotation(0,2)*point[2]+translation[0];
        point3d[1] =rotation(1,0)*point[0]+rotation(1,1)*point[1]+rotation(1,2)*point[2]+translation[1];
        point3d[2] =rotation(2,0)*point[0]+rotation(2,1)*point[1]+rotation(2,2)*point[2]+translation[2];

        const T x=point3d[0]/point3d[2];
        const T y=point3d[1]/point3d[2];
        //三维点重投影计算的像素坐标
//        Mat K = ( Mat_<double> ( 3,3 ) << 535.4, 0, 320.1, 0, 539.2,247.6, 0, 0, 1 );
//        const T u=x*535.4+320.1;
//        const T v=y*539.2+247.6;
//        cv::Mat K = ( cv::Mat_<double> ( 3,3 ) << 611.569, 0, 380.919, 0, 611.569,219.402, 0, 0, 1 );
//        const T u=x*420.0+320.0;
//        const T v=y*420.0+240.0;

        const T u=x*611.569+380.919;
        const T v=y*611.569+219.402;

        //观测的在图像坐标下的值
        T u1=T(observed_x);
        T v1=T(observed_y);

        residuals[0]=u-u1;
        residuals[1]=v-v1;
        return true;
    }

    static ceres::CostFunction* Create(const double observed_x, const double observed_y,Eigen::Isometry3d cameraPose){
        return (new ceres::AutoDiffCostFunction<MappointsCostFunction,2,3>(
                new MappointsCostFunction(observed_x,observed_y,cameraPose)));
    }


private:
    double observed_x;
    double observed_y;
    Eigen::Isometry3d pose_c;
};
#endif
